import cv2 as cv
import numpy as np
import pandas as pd

# Camera类所有的参数
"""
这是相机的类
创建这个类需要传入两个参数
file_path:相机配置文件
image_size:图片分辨率
"""
class Camera:
    def __init__(self, file_path, image_size):
        self.image_size = image_size
        self.file_path = file_path
        #读取表格文件
        df = pd.read_excel(self.file_path, header=None, engine="xlrd")
        """
        left_camera_matrix/right_camera_matrix:内参矩阵
        将3D点（相机坐标系）投影到2D图像平面
        [f_x, 0,   cx]  f_x, f_y：焦距（像素单位）
        [0,   f_y, cy]  cx, cy：光心坐标（图像中心）
        [0,   0,   1 ]
        
        left_distortion/right_distortion：畸变系数
        矫正镜头畸变（如鱼眼、桶形畸变）
        
        旋转矩阵R：3x3矩阵，描述右相机相对于左相机的旋转。
        平移向量T：3x1向量，描述右相机相对于左相机的平移（单位：米）。
        作用：定义两个相机的空间相对位置。
        """
        # 解析左相机参数，内参矩阵和畸变系数
        self.left_camera_matrix = np.array(df.iloc[0:3, 1:4], dtype=np.float64)
        self.left_distortion = np.array(df.iloc[5, 1:6], dtype=np.float64).reshape(1, 5)

        # 解析右相机参数，内参矩阵和畸变系数
        self.right_camera_matrix = np.array(df.iloc[6:9, 1:4], dtype=np.float64)
        self.right_distortion = np.array(df.iloc[11, 1:6], dtype=np.float64).reshape(1, 5)

        # 旋转平移矩阵 ,两个相机坐标相对关系
        self.T = np.array(df.iloc[12, 1:4], dtype=np.float64)  # 平移向量
        self.R = np.array(df.iloc[13:16, 1:4], dtype=np.float64)  # 旋转矩阵

        """
        计算矫正参数，包括映射矩阵和重投影矩阵
        R 左右相机旋转矫正矩阵：使左右图像对齐
        P 左右相机投影矩阵：3D点正确投影到2D图像上
        Q 重投影矩阵：计算三维点云
        映射矩阵 (left_map1, left_map2 等)通过remap()函数将原始畸变图像转换为矫正后的图像。
        """
        self.R1, self.R2, self.P1, self.P2, self.Q, _, _ = cv.stereoRectify(
            self.left_camera_matrix, self.left_distortion,
            self.right_camera_matrix, self.right_distortion,
            self.image_size, self.R, self.T
        )

        # 计算映射矩阵：存储像素变换关系
        self.left_map1, self.left_map2 = cv.initUndistortRectifyMap(
            self.left_camera_matrix, self.left_distortion, self.R1, self.P1, self.image_size, cv.CV_16SC2
        )
        self.right_map1, self.right_map2 = cv.initUndistortRectifyMap(
            self.right_camera_matrix, self.right_distortion, self.R2, self.P2, self.image_size, cv.CV_16SC2
        )

        """
        相机参数
        f_x,f_y:相机像素焦距
        cx,xy:光心坐标
        b:基线长度
        """
        self.f_x = self.Q[2, 3]
        self.f_y = self.Q[2, 3]
        self.cx = -self.Q[0, 3]
        self.cy = -self.Q[1, 3]
        self.b = (1 / self.Q[3, 2]) / 1000  # 基线长度（米）
